/*
    This file is part of ciberRatoToolsSrc.

    Copyright (C) 2001-2008 Universidade de Aveiro

    ciberRatoToolsSrc is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    ciberRatoToolsSrc is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Foobar; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

/* robview.cpp
 *
 * Widget that displays robot sensors
 *
 * For more information about the CiberRato Robot Simulator 
 * please see http://microrato.ua.pt/ or contact us.
 */

#include <math.h>
#include <iostream>
using std::cerr;

#include "RobSock.h"
#include "robview.h"

#define NUM_IR_SENSORS 4

#ifndef M_PI
#ifdef MicWindows
#define M_PI 3.14159265358979
#endif
#endif


SensorObstItem::SensorObstItem(QCanvas *canvas, 
                               int posx, int posy, double dir, int un)
                               : QCanvasEllipse(0,0,canvas)
{
        move(posx,posy);
        setAngles((int)((dir+60)*16),60*16);
        setBrush(QBrush("Green"));
        setVisible(true);
        setZ(12);

	unit=un;
	Dist=0.0;
}

#define MAXDIST 1.0 

void SensorObstItem::setDist(double measure) 
{
    double NewDist;

    if(measure>1/MAXDIST) NewDist=1/measure*unit;
    else NewDist=MAXDIST*unit;
    if(NewDist!=Dist)
    {
        setSize((int)(NewDist*2),(int)(NewDist*2));
	Dist=NewDist;
    }
}

RobCanvas::RobCanvas(double irSensorAngles[NUM_IR_SENSORS])
{
	unit=80;
        canvas_sizex=3*unit;
        canvas_sizey=(int)(3*unit);
	rob_radius=unit/2;

	rob_centerx=canvas_sizex/2;
	//rob_centery=canvas_sizey-rob_radius;
	rob_centery=canvas_sizey/2;

        resize(canvas_sizex,canvas_sizey);
        setBackgroundColor(QColor("White"));

        QCanvasEllipse *rob=new QCanvasEllipse(rob_radius*2,rob_radius*2,this);
        rob->move(rob_centerx,rob_centery);
        rob->setBrush(QBrush("Blue"));
        rob->setVisible(true);

        compassLine=new QCanvasLine(this);
        compassLine->setPoints(rob_centerx,rob_centery,
	                       rob_centerx,rob_centery);
        compassLine->setZ(10);
        compassLine->setVisible(true);

	for(int b=0; b<GetNumberOfBeacons();b++) {
           beaconLine.push_back(new QCanvasLine(this));
           beaconLine[b]->setPoints(rob_centerx,rob_centery,
	                            rob_centerx,rob_centery);
           beaconLine[b]->setZ(11);
	   beaconLine[b]->setPen(QPen("Yellow"));
           beaconLine[b]->setVisible(true);
	}

	for(unsigned int i=0; i < NUM_IR_SENSORS; i++)
           sensorObst[i]=new SensorObstItem(this,
	                                   (int)(rob_centerx-rob_radius*sin(irSensorAngles[i]/180.0*M_PI)),
				           (int)(rob_centery-rob_radius*cos(irSensorAngles[i]/180.0*M_PI)),
	                                   irSensorAngles[i],unit);
        
	setDoubleBuffering(true);
}

void RobCanvas::redrawRobot(void)
{
    if(IsCompassReady()) {
        double compass=GetCompassSensor()/180.0*M_PI;
        compassLine->setPoints(rob_centerx,rob_centery,
                           (int)(rob_centerx-rob_radius*sin(-compass)),
			   (int)(rob_centery-rob_radius*cos(-compass)));
	compassLine->setVisible(true);
    }
    else
	compassLine->setVisible(false);

    for(int b=0; b<GetNumberOfBeacons(); b++) {
	//cerr << "redrawRobot beacon " << b << " of " << GetNumberOfBeacons() << " ready " << IsBeaconReady(b) << " visible " <<GetBeaconSensor(b).beaconVisible << "\n";
        if(IsBeaconReady(b) && GetBeaconSensor(b).beaconVisible) {
	    //cerr << "redrawRobot seeing beacon " << b << " at " << GetBeaconSensor(b).beaconDir << "\n";
            double beacon=GetBeaconSensor(b).beaconDir/180.0*M_PI;
            beaconLine[b]->setPoints(rob_centerx,rob_centery,
                              (int)(rob_centerx-rob_radius*sin(beacon)),
	    		      (int)(rob_centery-rob_radius*cos(beacon)));
	    beaconLine[b]->setVisible(true);
        }
        else beaconLine[b]->setVisible(false);
    }

    for(unsigned int i = 0; i < NUM_IR_SENSORS; i++) {
       if(IsObstacleReady(i)) {
           double val=GetObstacleSensor(i);
           sensorObst[i]->setDist(val);
	   sensorObst[i]->setVisible(true);
       }
       else sensorObst[i]->setVisible(false);
    }
    
    setAllChanged(); // some versions of Qt might not need this
    update();
}

RobView::RobView(double irSensorAngles[NUM_IR_SENSORS], char *robName)
{
     setCaption(robName);
     robCanvas=new RobCanvas(irSensorAngles);
     setCanvas(robCanvas);
     resize(sizeHint());
     setMaximumSize(sizeHint());
     setMinimumSize(sizeHint());
}

void RobView::redrawRobot(void)
{
     robCanvas->redrawRobot();
}
